//
#include "MotorCtrl.h"
//
//*****************************************************************************
//!\brief   The TIM6 interrupt service function
//!\param   None
//!\retval  None                                                              
//*****************************************************************************
void TMR6_GLOBAL_IRQHandler(void)   // T = 50us
{
    /* Clear Update pending interrupt bit */
    TMR6->STS &= (uint16_t)~TMR_FLAG_Update;
    //
    g_Cnt1ms++;
    if(g_Cnt1ms >= 20)              // 20 * 50us = 1ms
    {
        g_Cnt1ms = 0;
        flag.Sys1ms =  1;
    }
}

//*****************************************************************************
//!\brief   The Break interrupt service function
//!\param   None
//!\retval  None                                                              
//*****************************************************************************
#if OCP_HW_EN
void TMR1_BRK_UP_TRG_COM_IRQHandler(void)
{
    /* Clear BRK_UP pending interrupt bit */
    TMR1->STS = (uint16_t)~TMR_FLAG_Break;
    //
	if(g_Stage == SECOND_STAGE)
	{
		EnterNoneStage();                            // Enter into the NONE stage !!!
        //
        TMR_CtrlPWMOutputs(TMR1, ENABLE);            // TMR1 Main Output Enable
        //
        protect.bitprt.ocp = 1;
        //
        #if DEBUG_INFOR_OUTPUT
        {
            printf("OCP_HW occurred!!\r\n");
        }
        #endif
	}
	else
	{
		TMR_CtrlPWMOutputs(TMR1, ENABLE);            // TMR1 Main Output Enable
	}
    //
    IWDG_ReloadCounter();
}
#endif

//*****************************************************************************
//!\brief   The TIM3 interrupt service function
//!\param   None
//!\retval  None                                                              
//*****************************************************************************
void TMR3_GLOBAL_IRQHandler(void)
{
    TMR3->STS  = 0;
    //
    InsertVector();
}

//*****************************************************************************
//!\brief   The ADC interrupt service function
//!\param   None
//!\retval  None                                                               
//*****************************************************************************
 void ADC_COMP_IRQHandler(void)
 {
    ADC1->STS = ~(uint32_t)ADC_FLAG_JEC;
    //
    if(g_ADC_ExternalTrigSource == ADC_ExternalTrigInjec_TMR1_CC4)
    {
        SET_ADC_ExternalTrigInjec_(ADC_ExternalTrigInjec_TMR3_CC4_ADC12);
        g_ADC_ExternalTrigSource = ADC_ExternalTrigInjec_TMR3_CC4_ADC12;
        //
        g_CurrIBusBuf[0] = (ADC1->JDOR1);                               // Get injected channel converted value.
        //
        ADC1->CTRL2 |= ADC_CTRL2_SWSTR;
    }
    else
    {
        SET_ADC_ExternalTrigInjec_(ADC_ExternalTrigInjec_TMR1_CC4);
        g_ADC_ExternalTrigSource = ADC_ExternalTrigInjec_TMR1_CC4;
        //
        g_CurrIBusBuf[1] = (ADC1->JDOR1);                               // Get injected channel converted value.
        //
        g_CntPWM++;
        //
        if(!protect.wordprt)
        {
            switch(g_Stage)  // Check Stage
            {
                case NONE_STAGE:
                    break;
                    //
                case FIRST_STAGE:
                    ResultFS = FirstStage();    // Offset Data
                    //
                    break;
                    //
                case SECOND_STAGE:              // FOC running
                    FOC_Core();
                    //
                    IBusUpdate();
                    //
                    #if (DEBUG_FOC_OUTPUT == 1)
                    {
                        DebugSave();
                    }
                    #endif
                    break;
                    //
                case BRAKE_STAGE:
                    ResultBS = BrakeStage();
                    //
                    break;
                    //
                default:
                    break;
            }
        }
        //
        while((DMA1->ISTS & DMA1_FLAG_TC1) == (uint32_t)RESET);
        DMA1->ICLR = DMA1_FLAG_TC1;
        //
        g_VoltageMosTemp = g_ADC_RegularConvertedValueTab[0];           // Get regular channel converted value.
        g_VoltageSpeed   = g_ADC_RegularConvertedValueTab[1];
        g_VoltageMotor   = g_ADC_RegularConvertedValueTab[2];
    }
}

//*****************************************************************************
//!\brief   The FOC Core Logic
//!\param   None
//!\retval  None
//*****************************************************************************
void FOC_Core(void)
{static uSHORT  cycle = 0;
static SHORT   angle_step = 0;
//
    //===============Get Phase Current======================================
    Curr_A_B = GetCurr_SingleShunt(g_CurrIBusBuf[0],g_CurrIBusBuf[1],g_CurrIBus_Offset,CURR_OFFSET_NUM);
    //
    //===============Clarke==================================================
    Curr_ALFA_BETA = Clarke(Curr_A_B);
    //
    //===============Park====================================================
    Curr_D_Q = Park(Curr_ALFA_BETA,Trig);
    //
    //===============Check FOC Index=========================================
    switch(INDEX_FOC)
    {
        case LOCATION: // Location
            //===============SMC=============================================
            SlideModeCtrl();
            //===============ID PID==========================================
            #if (ID_PID_MODE == 1)
            {
                Curr_D_PID_Func();
            }
            #endif
            //===============IQ PID==========================================
            #if (IQ_PID_MODE == 1)
            {
                Curr_Q_PID_Func();
            }
            #endif
            //==================Start Location=====================================
            if(g_CntPWM <= ((LOCATION_TIME >> 1) * ((uCHAR)(g_PWM_Frequency/1000))))
            {
                g_Angle = (SHORT)LOCATION_ANGLE;
            }
            else
            {
                if(g_ForRevDir == 0)
                {
                    g_Angle = (SHORT)LOCATION_ANGLE + (SHORT)5461;
                }
                else
                {
                    g_Angle = (SHORT)LOCATION_ANGLE - (SHORT)5461;
                }
            }
            //===================End Location======================================
            if(g_CntPWM >= (LOCATION_TIME * ((uCHAR)(g_PWM_Frequency/1000))))
            {
                g_CntPWM = 0;
                //
                angle_step = OPENLP_ANGLE_MIN;
                cycle = 0;
                //
                INDEX_FOC = OPEN_LOOP;
            }
            //
            break;
            //
        case OPEN_LOOP: // Open Loop
            //===============Check the sector=================================
            CheckSector();
            //===============Adaptive LPF Coefficient=========================
            AdapLPFCoef(&SMC.LPFCoeff);
            //===============SMC==============================================
            SlideModeCtrl();
            //===============Updating speed===================================
            SpeedUpdate();
            //===============ID PID===========================================
            #if (ID_PID_MODE == 1)
            {
                Curr_D_PID_Func();
            }
            #endif
            //===============IQ PID===========================================
            #if (IQ_PID_MODE == 1)
            {
                Curr_Q_PID_Func();
            }
            #endif
            //===============cycle and angle step=============================
            if(flag.open_loop_time)
            {
                flag.open_loop_time = 0;
                //
                cycle++;
                if(cycle >= OPENLP_CYCLE_MAX)
                {
                    cycle = OPENLP_CYCLE_MAX;
                }
                //
                angle_step += (OPENLP_ANGLE_STEP * cycle);
                if(angle_step >= OPENLP_ANGLE_MAX)
                {
                    angle_step = OPENLP_ANGLE_MAX;
                }
            }
            //===============Open Loop========================================
            if(g_ForRevDir == 0)
            {
                g_Angle += angle_step;
            }
            else
            {
                g_Angle -= angle_step;
            }
            //===============Open Loop to Closed Loop========================
            if(cycle >= OPENLP_CYCLE_MAX)
            {
                //===============Speed PID========================================
                #if (SPEED_IBUS_PID_MODE == 1)
                {
                    Speed_PID_Func();
                }
                //===============IBus PID=========================================
                #elif (SPEED_IBUS_PID_MODE == 2)
                {
                    IBus_PID_Func();
                }
                #endif
                //
                if(TRUE == OpenLpToClosedLp(&g_Angle,SMC.Angle,OPENLP_LPFCOEFF_STEP))
                {
                    INDEX_FOC = CLOSED_LOOP;
                }
            }
            //
            break;
            //
        case CLOSED_LOOP: // Closed Loop
            //===============Check the sector=================================
            CheckSector();
            //===============Adaptive LPF Coefficient=========================
            AdapLPFCoef(&SMC.LPFCoeff);
            //===============SMC==============================================
            SlideModeCtrl();
            //==============Angle=============================================
            g_Angle = SMC.Angle;
            //===============Updating speed===================================
            SpeedUpdate();
            //===============Speed PID========================================
            #if (SPEED_IBUS_PID_MODE == 1)
            {
                Speed_PID_Func();
            }
            //===============IBus PID=========================================
            #elif (SPEED_IBUS_PID_MODE == 2)
            {
                IBus_PID_Func();
            }
            #endif
            //===============ID PID===========================================
            #if (ID_PID_MODE == 1)
            {
                Curr_D_PID_Func();
            }
            #endif
            //===============IQ PID===========================================
            #if (IQ_PID_MODE == 1)
            {
                Curr_Q_PID_Func();
            }
            #endif
            //
            break;
            //
        default:
            break;
    }
    //
    //===============Sin & Cos============================================
    Trig = TrigFunc(g_Angle);
    //
    //===============Circle Limitation====================================
    Volt_D_Q = CircleLimit(Volt_D_Q,CircleModulation,CircleStartIndex);
    //
    //===============InvePark=============================================
    Volt_ALFA_BETA = InvePark(Volt_D_Q,Trig);
    //
    //===============SVPWM================================================
    SVPWM = SVPWM_Ctrl(Volt_ALFA_BETA,g_PWM_Period,g_PWMLoadData,TIME_MIN,TIME_DELAY_RATIO);
}

//*****************************************************************************
//!\brief   When debugging, save data
//!\param   None
//!\retval  None
//*****************************************************************************
void DebugSave(void)
{
    #if (DEBUG_FOC_OUTPUT == 1)
    {
        if(!flag.debug_save_ok)
        {
            #if (DB_FOC_DATA_TYPE == 0) // Phase Current
            DebugBuf_0[DBCnt] =  g_CurrIBusBuf[0];
            DebugBuf_1[DBCnt] =  g_CurrIBusBuf[1];
            DebugBuf_2[DBCnt] =  g_CurrIBus_Offset;
            DebugBuf_3[DBCnt] =  g_CurrIBusAVG;
            //
            #elif (DB_FOC_DATA_TYPE == 1) // Electrical Angle 
            DebugBuf_0[DBCnt] = g_Angle;
            DebugBuf_1[DBCnt] = SMC.Angle;
            DebugBuf_2[DBCnt] = 0;
            DebugBuf_3[DBCnt] = 0;
            //
            #elif (DB_FOC_DATA_TYPE == 2) // Clarke
            DebugBuf_0[DBCnt] = Curr_A_B.IComp_1;
            DebugBuf_1[DBCnt] = Curr_A_B.IComp_2;
            DebugBuf_2[DBCnt] = Curr_ALFA_BETA.IComp_1;
            DebugBuf_3[DBCnt] = Curr_ALFA_BETA.IComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 3) // Park
            DebugBuf_0[DBCnt] = Curr_ALFA_BETA.IComp_1;
            DebugBuf_1[DBCnt] = Curr_ALFA_BETA.IComp_2;
            DebugBuf_2[DBCnt] = Curr_D_Q.IComp_1;
            DebugBuf_3[DBCnt] = Curr_D_Q.IComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 4) // Speed PID
            DebugBuf_0[DBCnt] = g_SetSpeed;
            DebugBuf_1[DBCnt] = g_ActualSpeed;
            DebugBuf_2[DBCnt] = g_Speed_PID_Data;
            DebugBuf_3[DBCnt] = Curr_D_Q_REF.IComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 5) // IBus PID
            DebugBuf_0[DBCnt] = g_SetIBus;
            DebugBuf_1[DBCnt] = g_ActualIBus;
            DebugBuf_2[DBCnt] = g_IBus_PID_Data;
            DebugBuf_3[DBCnt] = Curr_D_Q_REF.IComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 6) // ID PID
            DebugBuf_0[DBCnt] = Curr_D_Q_REF.IComp_1;
            DebugBuf_1[DBCnt] = Curr_D_Q.IComp_1;
            DebugBuf_2[DBCnt] = g_Curr_D_PID_Data;
            DebugBuf_3[DBCnt] = Volt_D_Q.VComp_1;
            //
            #elif (DB_FOC_DATA_TYPE == 7) // IQ PID
            DebugBuf_0[DBCnt] = Curr_D_Q_REF.IComp_2;
            DebugBuf_1[DBCnt] = Curr_D_Q.IComp_2;
            DebugBuf_2[DBCnt] = g_Curr_Q_PID_Data;
            DebugBuf_3[DBCnt] = Volt_D_Q.VComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 8) // IPark
            DebugBuf_0[DBCnt] = Volt_D_Q.VComp_1;
            DebugBuf_1[DBCnt] = Volt_D_Q.VComp_2;
            DebugBuf_2[DBCnt] = Volt_ALFA_BETA.VComp_1;
            DebugBuf_3[DBCnt] = Volt_ALFA_BETA.VComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 9) // Sector
            DebugBuf_0[DBCnt] = SVPWM.Sector;
            DebugBuf_1[DBCnt] = SVPWM.TMR1_CC1;
            DebugBuf_2[DBCnt] = SVPWM.TMR1_CC2;
            DebugBuf_3[DBCnt] = SVPWM.TMR1_CC3;
            //
            #elif (DB_FOC_DATA_TYPE == 10) // Eα:Current Observer
            DebugBuf_0[DBCnt] = Volt_ALFA_BETA.VComp_1;
            DebugBuf_1[DBCnt] = MDM.BEMF_First.VComp_1;
            DebugBuf_2[DBCnt] = SMC.CorrFactor_1;
            DebugBuf_3[DBCnt] = MDM.CurrEst.IComp_1;
            //
            #elif (DB_FOC_DATA_TYPE == 11) // Eα：Bang-Bang Controller
            DebugBuf_0[DBCnt] = Curr_ALFA_BETA.IComp_1;
            DebugBuf_1[DBCnt] = MDM.CurrEst.IComp_1;
            DebugBuf_2[DBCnt] = SMC.CorrFactor_1;
            DebugBuf_3[DBCnt] = 0;
            //
            #elif (DB_FOC_DATA_TYPE == 12)  // Eα:LPF
            DebugBuf_0[DBCnt] = SMC.CorrFactor_1;
            DebugBuf_1[DBCnt] = MDM.BEMF_First.VComp_1;
            DebugBuf_2[DBCnt] = MDM.BEMF_Second.VComp_1;
            DebugBuf_3[DBCnt] = SMC.LPFCoeff;
            //
            #elif (DB_FOC_DATA_TYPE == 13) // Eβ:Current Observer
            DebugBuf_0[DBCnt] = Volt_ALFA_BETA.VComp_2;
            DebugBuf_1[DBCnt] = MDM.BEMF_First.VComp_2;
            DebugBuf_2[DBCnt] = SMC.CorrFactor_2;
            DebugBuf_3[DBCnt] = MDM.CurrEst.IComp_2;
            //
            #elif (DB_FOC_DATA_TYPE == 14) // Eβ:Bang-Bang Controller
            DebugBuf_0[DBCnt] = Curr_ALFA_BETA.IComp_2;
            DebugBuf_1[DBCnt] = MDM.CurrEst.IComp_2;
            DebugBuf_2[DBCnt] = SMC.CorrFactor_2;
            DebugBuf_3[DBCnt] = 0;
            //
            #elif (DB_FOC_DATA_TYPE == 15) // Eβ:LPF
            DebugBuf_0[DBCnt] = SMC.CorrFactor_2;
            DebugBuf_1[DBCnt] = MDM.BEMF_First.VComp_2;
            DebugBuf_2[DBCnt] = MDM.BEMF_Second.VComp_2;
            DebugBuf_3[DBCnt] = SMC.LPFCoeff;
            //
            #elif (DB_FOC_DATA_TYPE == 16) // Definition by yourself
            DebugBuf_0[DBCnt] = 0;
            DebugBuf_1[DBCnt] = 0;
            DebugBuf_2[DBCnt] = 0;
            DebugBuf_3[DBCnt] = 0;
            //
            #endif
            //
            DBCnt++;
            if(DBCnt >= DB_FOC_DATA_SIZE)
            {
               DBCnt = 0;
               flag.debug_save_ok  = 1;
            }
        }
    }
    #endif
}

//*****************************************************************************
//!\brief   The PMSM contrl logic.
//!\param   None
//!\retval  None
//*****************************************************************************
void MotorCtrl(void)
{
    //=======================================================================
    //-------------------System ON / OFF--------------------------------
    //=======================================================================
    g_SetSpeed = ((g_VoltageSpeed  * MOTOR_SPEED_MAX) >> 12);   // Set speed
    //
    if(!flag.SysOnOff)
    {
        EnterNoneStage();
        //
        protect.bitprt.lrcp = 0;                        // Clear the lrcp protect flags
        protect.bitprt.lrpp = 0;                        // Clear the lrpp protect flags
        protect.bitprt.uvp = 0;                         // Clear the uvp protect flags
        protect.bitprt.ovp = 0;                         // Clear the ovp protect flags
        protect.bitprt.ocp = 0;                         // Clear the ocp protect flags
        protect.bitprt.otp = 0;                         // Clear the otp protect flags
		//
        g_CurrIBusAVG = 0;
        //
        if(g_SetSpeed >= MOTOR_SPEED_MIN && ((g_ForRevDirUpdate == 0) || (g_ForRevDirUpdate == 1)))
        {// system on
            flag.SysOnOff = 1;                          // system on
            //
            g_ForRevDir = g_ForRevDirUpdate;
            //
            #if (DEBUG_INFOR_OUTPUT == 1)
            {
                printf("//==============Start===============\r\n");
                printf("\r\nSystem ON\r\n\r\n");
                printf("g_ForRevDir = %d\r\n",g_ForRevDir);
            }
            #endif
            //
            EnterFirstStage();                          // Enter into the first stage !!!
        }
    }
    else
    {
        #if (MOTOR_DIR_MODE == 1)
        {
            if((g_SetSpeed <= (MOTOR_SPEED_MIN - (MOTOR_SPEED_MIN >> 3))) || (g_ForRevDirUpdate != g_ForRevDir))
            {// system off
                if(!protect.wordprt)
                {
                    EnterBrakeStage();                  // Enter into the brake stage !!!
                }
                else
                {
                    EnterNoneStage();                   // Enter into the NONE stage !!!
                    flag.SysOnOff = 0;                  // system off
                    //
                    #if (DEBUG_INFOR_OUTPUT == 1)
                    {
                        printf("\r\nSystem OFF\r\n\r\n");
                        printf("//==============End===============\r\n");
                    }
                    #endif
                }
            }
        }
        #else
        {
            if(g_SetSpeed <= (MOTOR_SPEED_MIN - (MOTOR_SPEED_MIN >> 3)))
            {// system off
                if(!protect.wordprt)
                {
                    EnterBrakeStage();                  // Enter into the brake stage !!!
                }
                else
                {
                    EnterNoneStage();                   // Enter into the NONE stage !!!
                    flag.SysOnOff = 0;                  // system off
                    //
                    #if (DEBUG_INFOR_OUTPUT == 1)
                    {
                        printf("\r\nSystem OFF\r\n\r\n");
                        printf("//==============End===============\r\n");
                    }
                    #endif
                }
            }
        }
        #endif
    }
    //=============================================================================
    //-------------------Check the working stage---------------------------
    //=============================================================================
    if(!protect.wordprt)
    {
        switch(g_Stage) // Check Stage
        {
            case NONE_STAGE:
                break;
            case FIRST_STAGE:
                if(ResultFS)
                {
                    ResultFS = FALSE;
                    //
                    #if DEBUG_INFOR_OUTPUT
                    {
                        printf("\r\nThe First Stage: OK\r\n");
                    }
                    #endif
                    //
                    EnterSecondStage();                 // Enter into the second stage !!!
                }
                //
                break;
                //
            case SECOND_STAGE:
                #if DEBUG_INFOR_OUTPUT
                {
                    printf("\r\nThe Second Stage:FOC running\r\n");
                }
                #endif
                //
                switch(INDEX_FOC)
                {
                    case LOCATION:
                        //
                        //
                        //
                        // Please add your other control logic source code  here
                        //
                        //
                        //
                        //
                        break;
                        //
                    case OPEN_LOOP:
                        //
                        #if(OPENLP_IDREF_MODE == 1)
                        {
                            if(g_CntPWM >= (OPENLP_IDREF_TIME * ((uCHAR)(g_PWM_Frequency/1000))))
                            {
                                g_CntPWM = 0;
                                //
                                if(g_ActualSpeed < (g_SpeedRangeMin * OPENLP_IDREF_SPEED_RANGE))
                                {
                                    Curr_D_Q_REF.IComp_1 += OPENLP_IDREF_STEP_INC;
                                    if(Curr_D_Q_REF.IComp_1 >= OPENLP_IDREF_MAX)
                                    {
                                        Curr_D_Q_REF.IComp_1 = OPENLP_IDREF_MAX;
                                    }
                                }
                                else
                                {
                                    Curr_D_Q_REF.IComp_1 -= OPENLP_IDREF_STEP_DEC;
                                    if(Curr_D_Q_REF.IComp_1 <= OPENLP_IDREF_MIN)
                                    {
                                        Curr_D_Q_REF.IComp_1 = OPENLP_IDREF_MIN;
                                    }
                                }
                            }
                        }
                        #endif
                        //
                        //
                        //
                        // Please add your other control logic source code  here
                        //
                        //
                        //
                        //
                        break;
                        //
                    case CLOSED_LOOP:
                        //
                        Curr_D_Q_REF.IComp_1 = 0;
                        //
                        //
                        //
                        //
                        // Please add your other control logic source code  here
                        //
                        //
                        //
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                #if (DEBUG_FOC_OUTPUT == 1)
                {
                    DebugPrint();
                }
                #endif
                //
                break;
                //
            case BRAKE_STAGE:
                if(ResultBS)
                {
                    ResultBS = FALSE;
                    //
                    EnterNoneStage();                   // Enter into the NONE stage !!!
                    flag.SysOnOff = 0;                  // system off
                    //
                    #if DEBUG_INFOR_OUTPUT
                    {
                        printf("\r\nThe Brake Stage: OK\r\n");
                        printf("\r\nSystem OFF\r\n\r\n");
                        printf("//==============End===============\r\n");
                    }
                    #endif
                }
                //
                break;
                //
            default:
                break;
        }
    }
}

//*****************************************************************************
//!\brief   Sliding Mode Controller
//!\param   VoltInput: the input voltage
//!\param   CurrInput: the input current
//!\retval  electrical angle
//*****************************************************************************
void SlideModeCtrl(void)
{
    // Eα Eβ
    //=====Current Observer=========
    MotorDigModel(&MDM.CurrEst,Volt_ALFA_BETA,MDM.BEMF_First,SMC.CorrFactor_1,SMC.CorrFactor_2,MDM.Gain_F_Quo,MDM.Gain_F_Rem,MDM.Gain_G_Quo,MDM.Gain_G_Rem);
    //
    //=====Bang-Bang Controller=====
    BangBangCtrl(&SMC.CorrFactor_1,&SMC.CorrFactor_2,MDM.CurrEst,Curr_ALFA_BETA,SMC.Gain_K,SMC.ErrorMax);
    //
    //=====Low Pass Filter==========
    LPF32_Double(&MDM.BEMF_First,SMC.CorrFactor_1,SMC.CorrFactor_2,SMC.LPFCoeff);
    //
    //=====Low Pass Filter==========
    LPF32_Double(&MDM.BEMF_Second,MDM.BEMF_First.VComp_1,MDM.BEMF_First.VComp_2,SMC.LPFCoeff);
    //
    //==========CORDIC==============
    SMC.Angle =  CORDIC(MDM.BEMF_Second);
    //
    //=======Angle Compensation=====
    SMC.Angle += AngleComp(); 
}

//*****************************************************************************
//!\brief   Calculate and update the IBus.
//!\param   None
//!\retval  None
//*****************************************************************************
SHORT AngleComp(void)
{SHORT angle_comp = 0;
//
    #if(CLOSEDLP_ANGLE_COMP_MODE == 1)
    {
        if(g_ForRevDir == 0)
        {
            angle_comp = CLOSEDLP_ANGLE_COMP;
        }

        else
        {
            angle_comp = -CLOSEDLP_ANGLE_COMP;
        }
    }
    #else
    {
        angle_comp = 0;
    }
    #endif
    //
    return (angle_comp);
}

//*****************************************************************************
//!\brief   Calculate and update the motor speed.
//!\param   None
//!\retval  None
//*****************************************************************************
void SpeedUpdate(void)
{SHORT  data16_tmp  = 0;
static SHORT  angle_bak = 0;
static LONG  angle_sum = 0;
static uSHORT  angle_cnt = 0;
LONG  speed_tmp = 0;
//==============================================================
    if(!flag.speed_fst)
    {
        flag.speed_fst = 1;
        //
        angle_bak = g_Angle;
        angle_sum = 0;
        angle_cnt = 0;
    }
    else
    {
        data16_tmp = g_Angle - angle_bak;
        angle_bak = g_Angle;
        //
        angle_sum += (LONG)data16_tmp;
        //
        angle_cnt++;
        if(angle_cnt >= SPEED_ANGLE_NUM)
        {
            angle_cnt = 0;
            //
            if(angle_sum < 0)
            {
                angle_sum = -angle_sum;
            }
            //
            speed_tmp = (SMC.KSpeed * angle_sum)/1000;          // update  speed
            angle_sum = 0;
            //
            LPF32(&g_ActualSpeed,speed_tmp,SPEED_LPF_COEFF);    // speed LPF
        }
    }
}

//*****************************************************************************
//!\brief   The PID function of speed
//!\param   None
//!\retval  None
//*****************************************************************************
void Speed_PID_Func(void)
{static uSHORT speed_PID_cnt = 0;
LONG data_tmp = 0;
//====================================
    speed_PID_cnt++;
    if(speed_PID_cnt >= g_Speed_PID_Cycle)
    {
        speed_PID_cnt = 0;
        //
        g_Speed_PID_Data = Speed_PID_Ctrl(g_SetSpeed,g_ActualSpeed);
        //
        data_tmp = (LONG)Curr_D_Q_REF.IComp_2;
        //
        if(g_ForRevDir == 0)
        {
            data_tmp += g_Speed_PID_Data;
            if(data_tmp <= IQ_MIN)
            {
                data_tmp = IQ_MIN;
            }
            else if(data_tmp >= IQ_MAX)
            {
                data_tmp = IQ_MAX;
            }
        }
        else
        {
            data_tmp -= g_Speed_PID_Data;
            if(data_tmp <= -IQ_MAX)
            {
                data_tmp = -IQ_MAX;
            }
            else if(data_tmp >= -IQ_MIN)
            {
                data_tmp = -IQ_MIN;
            }
        }
        //
        Curr_D_Q_REF.IComp_2 = (SHORT)data_tmp;
    }
}

//*****************************************************************************
//!\brief   The PID function of IBus
//!\param   None
//!\retval  None
//*****************************************************************************
void IBus_PID_Func(void)
{LONG data_tmp = 0;
//====================================
    g_SetIBus = SET_IBUS * 10;
    g_ActualIBus = (((uLONG)((g_CurrIBusAVG * 10 * 3300)/(IBUS_SAMPLE_RES * IBUS_OPAMP))) >> 12);
    g_IBus_PID_Data = IBus_PID_Ctrl(g_SetIBus,g_ActualIBus);
    //
    data_tmp = (LONG)Curr_D_Q_REF.IComp_2;
    //
    if(g_ForRevDir == 0)
    {
        data_tmp += g_IBus_PID_Data;
        if(data_tmp <= IQ_MIN)
        {
            data_tmp = IQ_MIN;
        }
        else if(data_tmp >= IQ_MAX)
        {
            data_tmp = IQ_MAX;
        }
    }
    else
    {
        data_tmp -= g_IBus_PID_Data;
        if(data_tmp <= -IQ_MAX)
        {
            data_tmp = -IQ_MAX;
        }
        else if(data_tmp >= -IQ_MIN)
        {
            data_tmp = -IQ_MIN;
        }
    }
    //
    Curr_D_Q_REF.IComp_2 = (SHORT)data_tmp;
}

//*****************************************************************************
//!\brief   The PID function of current D
//!\param   None
//!\retval  None
//*****************************************************************************
void Curr_D_PID_Func(void)
{LONG data_tmp = 0;
//====================================
    g_Curr_D_PID_Data = Curr_D_PID_Ctrl(Curr_D_Q_REF.IComp_1,Curr_D_Q.IComp_1);
    //
    data_tmp = (LONG)Volt_D_Q.VComp_1;
    //
    data_tmp += g_Curr_D_PID_Data;
    if(data_tmp <= SHORT_MIN)
    {
        data_tmp = SHORT_MIN;
    }
    else if(data_tmp >= SHORT_MAX)
    {
        data_tmp = SHORT_MAX;
    }
    //
    Volt_D_Q.VComp_1 = (SHORT)data_tmp;
}

//*****************************************************************************
//!\brief   The PID function of current Q 
//!\param   None
//!\retval  None
//*****************************************************************************
void Curr_Q_PID_Func(void)
{LONG data_tmp = 0;
//====================================
    g_Curr_Q_PID_Data = Curr_Q_PID_Ctrl(Curr_D_Q_REF.IComp_2,Curr_D_Q.IComp_2);
    //
    data_tmp = (LONG)Volt_D_Q.VComp_2;
    //
    data_tmp += g_Curr_Q_PID_Data;
    if(data_tmp <= SHORT_MIN)
    {
        data_tmp = SHORT_MIN;
    }
    else if(data_tmp >= SHORT_MAX)
    {
        data_tmp = SHORT_MAX;
    }
    //
    Volt_D_Q.VComp_2 = (SHORT)data_tmp;
}

//*****************************************************************************
//!\brief   Calculate and update the IBus.
//!\param   None
//!\retval  None
//*****************************************************************************
void IBusUpdate(void)
{uCHAR cnt = 0;
SHORT  Curr_tmp = 0;
uLONG  CurrPhaseW_Sum = 0,CurrPhaseNumSum = 0;
static uLONG  CurrPhaseW_Acc = 0,CurrPhaseNum_Acc = 0;
static uLONG  CurrPhaseW_Buf[6] = {0},CurrPhaseNumBuf[6] = {0};
static uLONG *_pCurrPhaseW_Buf = CurrPhaseW_Buf;
static uLONG *_pCurrPhaseNumBuf = CurrPhaseNumBuf;
static T_SECTOR  SectorBak_IBus = SECTOR_NONE;
//==============================================================
    if(!flag.ibus_fst)
    {
        flag.ibus_fst = 1;
        //
        CurrPhaseNum_Acc = 0;
        CurrPhaseW_Acc = 0;
        SectorBak_IBus = SECTOR_NONE;
        //
        for(cnt = 0;cnt < 6;cnt++)
        {
            *(_pCurrPhaseNumBuf + cnt) = 0;
            *(_pCurrPhaseW_Buf + cnt) = 0;
        }
        //
        g_CurrIBusAVG = 0;
    }
    else
    {
        CurrPhaseNum_Acc++;
        //
        Curr_tmp = (Curr_A_B.IComp_1 / CURR_OFFSET_NUM);
        if(Curr_tmp < 0)
        {
            Curr_tmp = -Curr_tmp;
        }
        CurrPhaseW_Acc += Curr_tmp;
        //
        if(SectorBak_IBus != SVPWM.Sector)
        {
            SectorBak_IBus = SVPWM.Sector;
            // IBus
            for(cnt = 0;cnt < 5;cnt++)
            {
                *(_pCurrPhaseNumBuf + cnt) = *(_pCurrPhaseNumBuf + (cnt + 1));
                CurrPhaseNumSum += *(_pCurrPhaseNumBuf + cnt);
                //
                *(_pCurrPhaseW_Buf + cnt) = *(_pCurrPhaseW_Buf + (cnt + 1));
                CurrPhaseW_Sum += *(_pCurrPhaseW_Buf + cnt);
            }
            *(_pCurrPhaseNumBuf + 5) = CurrPhaseNum_Acc;
            CurrPhaseNum_Acc = 0;
            CurrPhaseNumSum += *(_pCurrPhaseNumBuf + 5);
            //
            *(_pCurrPhaseW_Buf + 5) = CurrPhaseW_Acc;
            CurrPhaseW_Acc = 0;
            CurrPhaseW_Sum += *(_pCurrPhaseW_Buf + 5);
            //
            g_CurrIBusAVG = (CurrPhaseW_Sum / CurrPhaseNumSum);
        }
    }
}

//*****************************************************************************
//!\brief   Check the sector.
//!\param   None
//!\retval  None
//*****************************************************************************
void  CheckSector(void)
{static CHAR sector_cnt = 0;
static T_SECTOR  sector_bak = SECTOR_NONE;
//==============================================================
    if(!flag.check_sector_fst)
    {
        flag.check_sector_fst = 1;
        flag.check_sector_ok = 0;
        //
        sector_cnt = 0;
        sector_bak = SECTOR_NONE;
    }
    else
    {
        if(sector_bak != SVPWM.Sector)
        {
            switch(sector_bak)
            {
                case SECTOR_1:
                    if(SVPWM.Sector == SECTOR_2)
                    {
                        sector_cnt++;
                    }
                    else if(SVPWM.Sector == SECTOR_6)
                    {
                        sector_cnt--;
                    }
                    else
                    {
                        sector_cnt = 0;
                    }
                    //
                    break;
                    //
                case SECTOR_2:
                    if(SVPWM.Sector == SECTOR_3)
                    {
                        sector_cnt++;
                    }
                    else if(SVPWM.Sector == SECTOR_1)
                    {
                        sector_cnt--;
                    }
                    else
                    {
                        sector_cnt = 0;
                    }
                    //
                    break;
                    //
                case SECTOR_3:
                    if(SVPWM.Sector == SECTOR_4)
                    {
                        sector_cnt++;
                    }
                    else if(SVPWM.Sector == SECTOR_2)
                    {
                        sector_cnt--;
                    }
                    else
                    {
                        sector_cnt = 0;
                    }
                    //
                    break;
                    //
                case SECTOR_4:
                    if(SVPWM.Sector == SECTOR_5)
                    {
                        sector_cnt++;
                    }
                    else if(SVPWM.Sector == SECTOR_3)
                    {
                        sector_cnt--;
                    }
                    else
                    {
                        sector_cnt = 0;
                    }
                    //
                    break;
                    //
                case SECTOR_5:
                    if(SVPWM.Sector == SECTOR_6)
                    {
                        sector_cnt++;
                    }
                    else if(SVPWM.Sector == SECTOR_4)
                    {
                        sector_cnt--;
                    }
                    else
                    {
                        sector_cnt = 0;
                    }
                    //
                    break;
                    //
                case SECTOR_6:
                    if(SVPWM.Sector == SECTOR_1)
                    {
                        sector_cnt++;
                    }
                    else if(SVPWM.Sector == SECTOR_5)
                    {
                        sector_cnt--;
                    }
                    else
                    {
                        sector_cnt = 0;
                    }
                    //
                    break;
                    //
                default:
                    sector_cnt = 0;
                    //
                    break;
            }
            //
            sector_bak = SVPWM.Sector;
            //
            if(sector_cnt == 6 || sector_cnt == -6)
            {
                sector_cnt = 0;
                //
                flag.check_sector_ok = 1;
                //
                flag.open_loop_time = 1;
                flag.adp_LPF_coeff_en = 1;
                //
                flag.speed_update_ok = 1;
            }
        }
    }
}

//*****************************************************************************
//!\brief   When debugging, print data
//!\param   None
//!\retval  None
//*****************************************************************************
void DebugPrint(void)
{
    #if (DEBUG_FOC_OUTPUT == 1)
    {
        uSHORT cnt = 0;
        //
        if(flag.debug_save_ok)
        {
            printf("\r\n//===FOC Data Start===\r\n\r\n");
            for(cnt = 0;cnt < DB_FOC_DATA_SIZE; cnt++)
            {
                printf("%d\t%d\t%d\t%d\r\n",DebugBuf_0[cnt],DebugBuf_1[cnt],DebugBuf_2[cnt],DebugBuf_3[cnt]);
            }
            printf("\r\n//===FOC Data End===\r\n\r\n");
            //
            flag.debug_save_ok = 0;
        }
    }
    #endif
}

//*****************************************************************************
//!\brief	Enter into the none stage.
//!\param	None
//!\retval  None                                                             
//*****************************************************************************
void EnterNoneStage(void)
{
    if(g_Stage != NONE_STAGE)
    {
        g_Stage = NONE_STAGE;               // Enter into the NONE stage !!!
        //
        PWM_OnOff(OFF);                     // Close all the PWM
        //
        SET_TMR1_CCR_(0,0,0);
        SET_TMR1_CCR4_(1);
        SET_ADC_ExternalTrigInjec_(ADC_ExternalTrigInjec_TMR1_CC4);
        g_ADC_ExternalTrigSource = ADC_ExternalTrigInjec_TMR1_CC4;
        //
        SET_TMR3_CCR_(0,0,0);
        SET_TMR3_CCR4_((g_PWMLoadData - 1));
        TMR_INTConfig(TMR3,TMR_INT_CC1,DISABLE);
        TMR_INTConfig(TMR3,TMR_INT_CC2,DISABLE);
    }
}

//*****************************************************************************
//!\brief	Enter into the first stage.
//!\param	None
//!\retval  None                                                             
//*****************************************************************************
void EnterFirstStage(void)
{
    if(g_Stage != FIRST_STAGE)
    {
        EnterNoneStage();                   // Enter into the NONE stage !!!
        // Debug
        #if (DEBUG_FOC_OUTPUT == 1)
        {uSHORT cnt = 0;
        //
            flag.debug_save_ok = 0;
            DBCnt = 0;
            for(cnt = 0;cnt < DB_FOC_DATA_SIZE; cnt++)
            {
                DebugBuf_0[cnt] = 0;
                DebugBuf_1[cnt] = 0;
                DebugBuf_2[cnt] = 0;
                DebugBuf_3[cnt] = 0;
            }
        }
        #endif
        //
        g_CurrOffset_Cnt = 0;
        g_CurrIBus_Offset = 0;
        //
        INDEX_FS = INDEX_START;
        g_CntPWM = 0;
        ResultFS = FALSE;
        g_Stage = FIRST_STAGE;              // Enter into the first stage !!!
    }
}

//*****************************************************************************
//!\brief	Enter into the second stage.
//!\param	None
//!\retval  None                                                             
//*****************************************************************************
void EnterSecondStage(void)
{
    if(g_Stage != SECOND_STAGE)
    {
        EnterNoneStage();                   // Enter into the NONE stage !!!
        //==============================================================
        // Speed and PWM
        g_ActualSpeed = 0;
        g_SpeedRangeMin = (MOTOR_SPEED_MAX / 10);
        g_PWM_InverseIndex = 0;
        //==============================================================
        // FLAG
        flag.speed_update_ok = 0;
        //
        flag.check_sector_fst = 0;
        flag.check_sector_ok = 0;
        //
        flag.adp_LPF_coeff_fst = 0;
        flag.adp_LPF_coeff_en = 0;
        //
        flag.open_loop_time = 0;
        //
        flag.ibus_fst = 0;
        flag.speed_fst = 0;
        //==============================================================
        // PID
        g_Speed_PID_Cycle = SPEED_PID_PERIOD;
        Speed_PID_Init(SPEED_PID_KP,SPEED_PID_KI,SPEED_PID_OUTPUT_MAX);
        Speed_PID_Rest();
        //
        IBus_PID_Init(IBUS_PID_KP,IBUS_PID_KI,IBUS_PID_OUTPUT_MAX);
        IBus_PID_Rest();
        //
        Curr_D_PID_Init(ID_PID_KP,ID_PID_KI,ID_PID_OUTPUT_MAX);
        Curr_D_PID_Rest();
        //
        Curr_Q_PID_Init(IQ_PID_KP,IQ_PID_KI,IQ_PID_OUTPUT_MAX);
        Curr_Q_PID_Rest();
        //
        //==============================================================
        // Clark and Park
        Curr_A_B.IComp_1 = 0;
        Curr_A_B.IComp_2 = 0;
        Curr_ALFA_BETA.IComp_1 = 0;
        Curr_ALFA_BETA.IComp_2 = 0;
        Curr_D_Q.IComp_1 = 0;
        Curr_D_Q.IComp_2 = 0;
        //==============================================================
        // ID start and IQ start
        Curr_D_Q_REF.IComp_1 = ID_START;
        //
        if(g_ForRevDir == 0)
        {
            Curr_D_Q_REF.IComp_2 = IQ_START;            // IQ start
        }
        else
        {
            Curr_D_Q_REF.IComp_2 = -IQ_START;           // IQ start
        }
        //==============================================================
        // IPark
        Volt_D_Q.VComp_1 = 0;
        Volt_D_Q.VComp_2 = 0;
        Volt_ALFA_BETA.VComp_1 = 0;
        Volt_ALFA_BETA.VComp_2 = 0;
        //==============================================================
        // sin cos
        Trig.Cos = 0;
        Trig.Sin = 0;
        CircleModulation = MODULATION_MAX;
        CircleStartIndex = START_INDEX;
        //==============================================================
        // SVPWM
        SVPWM.Sector = SECTOR_NONE;
        SVPWM.TrigType = TYPE_NONE;
        //
        SVPWM.TMR1_CC1 = 0;
        SVPWM.TMR1_CC2 = 0;
        SVPWM.TMR1_CC3 = 0;
        SVPWM.TMR1_CC4 = 0;
        //
        SVPWM.TMR3_CC1 = 0;
        SVPWM.TMR3_CC2 = 0;
        SVPWM.TMR3_CC4 = 0;
        //==============================================================
        // MDM
        MDM.Gain_F = (LONG)((1.0 - (FLOAT)((MOTOR_R * 1000.0) / (g_PWM_Frequency * MOTOR_L))) * 32768.0);
        MDM.Gain_F_Quo = (SHORT)(MDM.Gain_F / 32768);
        MDM.Gain_F_Rem = (SHORT)(MDM.Gain_F % 32768);
        MDM.Gain_G = (LONG)((FLOAT)(1000000.0 / (g_PWM_Frequency * MOTOR_L)) * 32768.0);
        MDM.Gain_G_Quo = (SHORT)(MDM.Gain_G / 32768);
        MDM.Gain_G_Rem = (SHORT)(MDM.Gain_G % 32768);
        MDM.CurrEst.IComp_1 = 0;
        MDM.CurrEst.IComp_2 = 0;
        MDM.BEMF_First.VComp_1 = 0;
        MDM.BEMF_First.VComp_2 = 0;
        MDM.BEMF_Second.VComp_1 = 0;
        MDM.BEMF_Second.VComp_2 = 0;
        //==============================================================
        // SMC 
        SMC.ErrorMax = SMC_ERROR_MAX;
        SMC.Gain_K = SMC_GAIN_K;
        SMC.CorrFactor_1 = 0;
        SMC.CorrFactor_2 = 0;
        SetLPFCoeff(&SMC.LPFCoeff,g_PWM_Frequency,g_MotorPolePairs,MOTOR_SPEED_MIN);
        SMC.Angle = 0;
        SMC.KSpeed = (uLONG)(60 * g_PWM_Frequency * 1000) / (uLONG)(65536 * SPEED_ANGLE_NUM * g_MotorPolePairs);
        //==============================================================
                //
        #if DEBUG_INFOR_OUTPUT
        {
            printf("\r\nGain_F= %d,Gain_F_Quo= %d,Gain_F_Rem= %d\r\n",MDM.Gain_F,MDM.Gain_F_Quo,MDM.Gain_F_Rem);
            printf("\r\nGain_G= %d,Gain_G_Quo= %d,Gain_G_Rem= %d\r\n",MDM.Gain_G,MDM.Gain_G_Quo,MDM.Gain_G_Rem);
            printf("\r\nGain_K= %d,ErrorMax = %d,LPFCoeff= %d,KSpeed= %d\r\n",SMC.Gain_K,SMC.ErrorMax,SMC.LPFCoeff,SMC.KSpeed);
        }
        #endif
        //==============================================================
        //
        PWM_OnOff(ON);
        //
        INDEX_FOC = LOCATION;
        g_CntPWM = 0;
        //
        g_Stage = SECOND_STAGE;                             // Enter into the second stage !!!
    }
}

//*****************************************************************************
//!\brief	Enter into the brake stage.
//!\param	None
//!\retval  None                                                             
//*****************************************************************************
void EnterBrakeStage(void)
{
    if(g_Stage != BRAKE_STAGE)
    {
        EnterNoneStage();                       // Enter into the NONE stage !!!
        //	the brake stage variable
        g_PWM_CCR_Duty = BS_CCR_START_DUTY;
        g_PWM_CCR_Data = CCR_DATA_(g_PWM_CCR_Duty);
        SET_TMR1_CCR_(g_PWM_CCR_Data,g_PWM_CCR_Data,g_PWM_CCR_Data);     // Set the PWM0,PWM1,PWM2 CCR data 
        //
        ResultBS = FALSE; 
        INDEX_BS = INDEX_START;
        g_Stage = BRAKE_STAGE;                      // Enter into the brake stage !!!
    }
}

//*****************************************************************************
//!\brief	The first stage contrl logic.
//!\param	None
//!\retval  TRUE/FALSE                                                                   
//*****************************************************************************
T_BOOL  FirstStage(void)
{
    if(INDEX_FS == INDEX_START)
    {
        if(g_CntPWM <= (20 * ((uCHAR)(g_PWM_Frequency/1000))))
        {
            PWM_OnOff(A_L_B_H);
        }
        else
        {
            PWM_OnOff(OFF);
            //
            INDEX_FS = INDEX_1;
        }
    }
    else if(INDEX_FS == INDEX_1)
    {
        if(g_CurrOffset_Cnt < CURR_OFFSET_NUM)
        {
            g_CurrOffset_Cnt++;
            //
            g_CurrIBus_Offset += g_CurrIBusBuf[1];
        }
        else
        {
           return TRUE;
        }
    }
    //
    return FALSE;
}

//*****************************************************************************
//!\brief	The brake contrl logic.
//!\param	None
//!\retval  TRUE/FALSE                                                                   
//*****************************************************************************
T_BOOL  BrakeStage(void)
{
    switch(INDEX_BS)
    {
        case INDEX_START:
            PWM_OnOff(A_L_B_PWM);
            //
            g_CntPWM = 0;
            INDEX_BS = INDEX_1;
            //
            break;
            //
        case INDEX_1:
            if(g_CntPWM == (BS_SD_TIME * ((uCHAR)(g_PWM_Frequency/1000))))
            {
                PWM_OnOff(A_L_B_H); 
            }
            else if(g_CntPWM  == (BS_SD_TIME + BS_EB_TIME) * ((uCHAR)(g_PWM_Frequency/1000)))
            {
                PWM_OnOff(OFF);
                g_CntPWM = 0;
                INDEX_BS = INDEX_END;
            }
            //
            break;
            //
        default:
            break;
    }
    //
    if(INDEX_BS == INDEX_END)
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

//*****************************************************************************
//!\brief	The PWM ON/OFF contrl logic.
//!\param	None
//!\retval  TRUE/FALSE                                                         
//*****************************************************************************
void PWM_OnOff(T_PWM_STATUS  onoff)
{static T_PWM_STATUS onoff_bak = NONE;
//
    if(onoff_bak != onoff)
    {
        onoff_bak = onoff;
        //
        switch(onoff_bak)
        {
            case ON:
                 M_PWM_OUTPUT_ON;
                break;
            case OFF:
                M_PWM_OUTPUT_OFF;
                break;
            case A_L_B_PWM:
                M_PWM_OUTPUT_A_L_B_PWM; 
                break;
            case A_L_B_H:
                M_PWM_OUTPUT_A_L_B_H;
                break;
            default:
                break;
        }
    }
}

//===========================================================================================

